#include "System.h"



//Simple Messaging Transport    --> MOdified for Big Endian!
//
//
// Byte 0 --> 0xAA          (Header)
// Byte 1 --> 0xAA          (Header)
// Byte 2 --> MessageLength (MSB)    : Length of Message
// Byte 3 --> MessageLength (LSB)    : 
// Byte [4 to 4 + MessageLength] (Message)
// Byte [MessageLength]  CRC (MSB) : CRC of Everything Header to Message.
// Byte [MessageLength+1]  CRC (LSB) : CRC of Everything Header to Message.
//
//CCITT 16bit algorithm (X^16 + X^12 + X^5 + 1)

IncomingMsg BackdoorMsg;

WORD MsgCRC;
WORD MsgIndex;
BYTE GetMsgStringLength(BYTE * String);
BYTE MsgStringLength;

AncillaryMsgData MyAncillaryMsgData;

BYTE OutgoingMsgBuf[MAX_MESSAGE_SIZE+MESSAGE_FRAMING_SIZE];
WORD OutgoingMsgLength;

 //Messages
SteeringServoCtrl ExternalSteeringServoCommand;
DriveServoCtrl    ExternalDriveServoCommand;

BYTE LastCentroidFilterLength = 255;

void InitMessageDetect()
{
	BackdoorMsg.DetectState = SCAN_FOR_HEADER1;
}



void ProcessMessage(IncomingMsg * IncomingMessage)
{
	switch(IncomingMessage->Payload[0])
	{
		
		case MSG_SOFTWARE_RESET:
				SBDFR_BDFR = 1;
				//Force a background debug reset.
		break;
	
	  case MSG_CHANGE_SYSTEM_STATE:

	    EnterSystemState(IncomingMessage->Payload[1]);

	  break;
	
	  case MSG_SET_DRIVE_SERVO:
	  
	     memcpy(&ExternalDriveServoCommand,&IncomingMessage->Payload[1],sizeof(DriveServoCtrl));
	     DriveServoNeutralPosition = ExternalDriveServoCommand.DriveServoNeutral;
	     SetDriveServo(ExternalDriveServoCommand.DriveServoVal);
	     
	  break;
	  
	  case MSG_SET_STEERING_SERVO:
	     memcpy(&ExternalSteeringServoCommand,&IncomingMessage->Payload[1],sizeof(DriveServoCtrl));
	     
	     SteeringServoNeutralPosition = ExternalSteeringServoCommand.SteeringServoNeutral;
	     SetSteeringServo(ExternalSteeringServoCommand.SteeringServoVal);
	  break;
	
    
	  case MSG_SET_AUTO_NV_PARAMETERS:

        LastCentroidFilterLength = MyAutoNVParameters.CentroidFilterLength;
        
      	 (void)memcpy(&MyAutoNVParameters,&IncomingMessage->Payload[1],sizeof(MyAutoNVParameters));
	      
	      if(LastCentroidFilterLength != MyAutoNVParameters.CentroidFilterLength)
	      {
	        CentroidFilterIndex = 0;
	       }
	  break;
	
	
	  case MSG_SIMULATE_BUTTON_PRESS:
	  
	        if(IncomingMessage->Payload[1] & 0x01)
	            ButtonPressed[0] = TRUE;
	        if(IncomingMessage->Payload[1] & 0x02)
	            ButtonPressed[1] = TRUE;
	        if(IncomingMessage->Payload[1] & 0x04)
	            ButtonPressed[2] = TRUE;
	        if(IncomingMessage->Payload[1] & 0x08)
	            ButtonPressed[3] = TRUE;
	        
	  break;
		case MSG_GET_FIRMWARE_VERSION:
					MyAncillaryMsgData.FirmwareVersion.FirmwareVersionMajor = FIRMWARE_VERSION_MAJOR;
					MyAncillaryMsgData.FirmwareVersion.FirmwareVersionMinor = FIRMWARE_VERSION_MINOR;
					(void)AssembleMessage(&OutgoingMsgBuf[0],MSG_REPORT_FIRMWARE_VERSION,&OutgoingMsgLength,&MyAncillaryMsgData);
					(void)SerialByteArrayEnqueue(&SCI1OutgoingQueue,&OutgoingMsgBuf[0],OutgoingMsgLength);
		break;
			
			default:
					AssembleMessage(&OutgoingMsgBuf[0],MSG_UNKNOWN_MESSAGE,&OutgoingMsgLength,NULL);
				  (void)SerialByteArrayEnqueue(&SCI1OutgoingQueue,&OutgoingMsgBuf[0],OutgoingMsgLength);
					break;	
	}	
}
	
		BYTE NextByteIn;
	  BYTE Len;	
	  BYTE Eli[64];
	  	
void ProcessIncomingBackdoorMessages()
{
		WORD i;
	
	  	for(i=0;i<BytesInQueue(&SCI1IncomingQueue);i++)
	  	{
	  		(void)ByteDequeue(&SCI1IncomingQueue,&NextByteIn);
	  		
	  		switch(MessageDetect(&BackdoorMsg,NextByteIn))
	  		{
	  			case MESSAGE_DETECTED:
	  				ProcessMessage(&BackdoorMsg);
	  			break;
	  			
	  			case BAD_CRC:
					(void)AssembleMessage(&OutgoingMsgBuf[0],MSG_BAD_CRC,&OutgoingMsgLength,NULL);
				  (void)SerialByteArrayEnqueue(&SCI1OutgoingQueue,&OutgoingMsgBuf[0],OutgoingMsgLength);
				break;
	  			
	  			case INVALID_MSG_LENGTH:
					(void)AssembleMessage(&OutgoingMsgBuf[0],MSG_INVALID_LENGTH,&OutgoingMsgLength,NULL);
					(void)SerialByteArrayEnqueue(&SCI1OutgoingQueue,&OutgoingMsgBuf[0],OutgoingMsgLength);
	  		
	  			break;
	  			
	  			default:
	  			break;
	  		}
	  	}
	 
}


BOOL AssembleMessage(BYTE *MsgBuf,BYTE MsgType,WORD *TotalMsgLength, void *AncillaryData)
{
	BOOL RetVal;
	BYTE i;
	
	switch(MsgType)
	{
	
	
	  case MSG_POST_AUTO_NV_STATE_VARIABLES:
	
	    MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
	  	MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] =  MESSAGE_ID_SIZE + sizeof(AutoNVStateVariables);
	  	MsgBuf[MsgIndex++] = MSG_POST_AUTO_NV_STATE_VARIABLES;
			 		
		  for(i=0;i<sizeof(AutoNVStateVariables);i++)
		  {
		  	MsgBuf[MsgIndex++] = ((BYTE *)AncillaryData)[i];
      }

			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
	  break;
	  
	  
	 		case MSG_POST_SENSOR_DATA:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
	  	MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] =  MESSAGE_ID_SIZE + sizeof(SensorData);
	  	MsgBuf[MsgIndex++] = MSG_POST_SENSOR_DATA;
			 		
		  for(i=0;i<sizeof(SensorData);i++)
		  {
		  	MsgBuf[MsgIndex++] = ((BYTE *)AncillaryData)[i];
      }

			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		
		case MSG_POST_SYSTEM_STATE:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] =  MESSAGE_ID_SIZE + sizeof(MyAncillaryMsgData.SystemState);
			MsgBuf[MsgIndex++] = MSG_POST_SYSTEM_STATE;
			MsgBuf[MsgIndex++] = ((AncillaryMsgData *)(AncillaryData))->SystemState.Current;


			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
		
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		case MSG_REPORT_FIRMWARE_VERSION:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] =  MESSAGE_ID_SIZE + sizeof(MyAncillaryMsgData.FirmwareVersion);
			MsgBuf[MsgIndex++] = MSG_REPORT_FIRMWARE_VERSION;
			MsgBuf[MsgIndex++] = ((AncillaryMsgData *)(AncillaryData))->FirmwareVersion.FirmwareVersionMajor;
			MsgBuf[MsgIndex++] = ((AncillaryMsgData *)(AncillaryData))->FirmwareVersion.FirmwareVersionMinor;
			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		case MSG_BAD_CRC:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] = MESSAGE_ID_SIZE;
			MsgBuf[MsgIndex++] = MSG_BAD_CRC;
			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		case MSG_INVALID_LENGTH:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] = MESSAGE_ID_SIZE;
	    MsgBuf[MsgIndex++] = MSG_INVALID_LENGTH;
			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
		MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		case MSG_UNKNOWN_MESSAGE:
			MsgIndex = 0;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
		
			MsgBuf[MsgIndex++] = MESSAGE_ID_SIZE;
			MsgBuf[MsgIndex++] = MSG_UNKNOWN_MESSAGE;
			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
			
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
		
		case MSG_STRING:
			MsgIndex = 0;
			MsgStringLength = GetMsgStringLength((BYTE *)AncillaryData);
			
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_1;
			MsgBuf[MsgIndex++] = MESSAGE_HEADER_2;
			MsgBuf[MsgIndex++] = 0x0;
			MsgBuf[MsgIndex++] = MsgStringLength + MESSAGE_ID_SIZE;
			MsgBuf[MsgIndex++] = MSG_STRING;
			
			for(i=0;i<MsgStringLength;i++)
			{
				MsgBuf[MsgIndex++] = ((BYTE *)AncillaryData)[i];
			}
			
			MsgCRC = CalcCRC( &MsgBuf[0], MsgIndex);
		
			MsgBuf[MsgIndex++] = (BYTE)(MsgCRC>>8);
			MsgBuf[MsgIndex++] = (BYTE)MsgCRC;
			*TotalMsgLength = MsgIndex;
			RetVal = TRUE;
		break;
	
		default:
			RetVal = FALSE;
		break;
		
	}
	
	return RetVal;
}

BYTE GetMsgStringLength(BYTE * String)
{
	BYTE i;
	i=0;
	while((i<=MAX_MESSAGE_STRING_LENGTH) && (String[i]!=0))
	{
		i++;	
	}
	return i;
}

unsigned char MessageDetect(IncomingMsg * IncomingMessage, unsigned char DataIn)
{
   //Detect Status will be our return value to indicate if a MESSAGE is
   //available to Process
   BYTE  DetectStatus = 0;
   WORD CRCTemp;
   //implement a message detection StateMachine
    
   switch(IncomingMessage->DetectState)
   {
      case SCAN_FOR_HEADER1:
         if(DataIn == MESSAGE_HEADER_1)
			{
            	IncomingMessage->DetectState = SCAN_FOR_HEADER2;
     			IncomingMessage->Header1 = DataIn;	
			}   
	 	 else
	        {
           		IncomingMessage->DetectState = SCAN_FOR_HEADER1;
            }
         		DetectStatus = NO_MESSAGE_DETECTED;
      break;

      case SCAN_FOR_HEADER2:
         if(DataIn == MESSAGE_HEADER_1)
		  {
            IncomingMessage->DetectState = GRAB_LENGTH_HIGH;
         	IncomingMessage->Header2 = DataIn;
	      }
		else
          {
            IncomingMessage->DetectState = SCAN_FOR_HEADER1;
          }
      break;
	
     case GRAB_LENGTH_HIGH:
              
                IncomingMessage->Length = ((WORD)DataIn)<<8;
		          IncomingMessage->DetectState = GRAB_LENGTH_LOW;
              DetectStatus = NO_MESSAGE_DETECTED;
      break;
      
	case GRAB_LENGTH_LOW:
            
			         IncomingMessage->DataCnt = 0;					
			        IncomingMessage->Length |= (WORD)DataIn;				
              if(IncomingMessage->Length > MAX_MESSAGE_SIZE)
              {
                DetectStatus = INVALID_MSG_LENGTH;
			    IncomingMessage->DetectState = SCAN_FOR_HEADER1;
              }
              else
              {
              	IncomingMessage->DetectState = GRAB_PAYLOAD;
              }
            break;
	  
      case GRAB_PAYLOAD:
              IncomingMessage->Payload[IncomingMessage->DataCnt]=DataIn;
               IncomingMessage->DataCnt++;
		      if( IncomingMessage->DataCnt == (IncomingMessage->Length))
                  IncomingMessage->DetectState = GRAB_CRC_HIGH;
              else
                  IncomingMessage->DetectState = GRAB_PAYLOAD;

              DetectStatus = NO_MESSAGE_DETECTED;
      break;

      case GRAB_CRC_HIGH:
              IncomingMessage->CRC = 0;
              IncomingMessage->CRC = (WORD)(DataIn<<8);
              IncomingMessage->DetectState = GRAB_CRC_LOW;
              DetectStatus =  NO_MESSAGE_DETECTED;
      break;
      
      case GRAB_CRC_LOW:
     		
             IncomingMessage->CRC |= DataIn;
             IncomingMessage->DetectState = SCAN_FOR_HEADER1;
				
			      CRCTemp =  CalcCRC(&IncomingMessage->Header1,(IncomingMessage->Length)+4);
             if(CRCTemp  == IncomingMessage->CRC)
               {
                   DetectStatus = MESSAGE_DETECTED;
                }
               else
               {
                  DetectStatus = BAD_CRC;
               }
 
			  
        break;

      default:
         IncomingMessage->DetectState = SCAN_FOR_HEADER1;
         DetectStatus = NO_MESSAGE_DETECTED;
      break;
   }

   return DetectStatus;

}





